#include "ros/ros.h"
#include <cv_bridge/cv_bridge.h>
#include <camera_info_manager/camera_info_manager.h>
#include "sensor_msgs/Image.h"
#include "sensor_msgs/CompressedImage.h"
#include "opencv2/opencv.hpp"
#include "s2250mid.h"

#include <sstream>
#include <vector>

using namespace cv;

int main(int argc, char **argv)
{
    frame_info_t fi;
    MIDSRAY_VIDSYS vidsys;
    int board;
    cv_bridge::CvImage cv_image;
    std::string frame = "camera";
    int fps = 30;

    std::string url;

    ros::init(argc, argv, "s2250_capture");

    ros::NodeHandle n("~");

    n.getParam("frame_id", frame);
    n.getParam("camera_info_url", url);
    n.getParam("fps", fps);

    camera_info_manager::CameraInfoManager info_mgr(n, "camera", url);

    ros::Publisher image_pub = n.advertise<sensor_msgs::Image>("image_raw", 1);
    ros::Publisher cmp_image_pub = n.advertise<sensor_msgs::CompressedImage>("image_raw/compressed", 1);
    ros::Publisher info_pub = n.advertise<sensor_msgs::CameraInfo>("camera_info", 1);

    ros::Rate loop_rate(fps);

    board = 0;

    vidsys = MIDSRAY_VIDSYS_NTSC;

    SN_OpenBoard(board);

    SN_SetBasicChipSettings(board, MIDSRAY_ENCODE_MOTIONJPEG, 0, vidsys, 4000000, 0);

    SN_StartStream( board);

    int result;
    Mat mat = cvCreateMat(480, 720, CV_8UC3);
    sensor_msgs::CompressedImage cmpImage;
    cmpImage.format = "jpeg";
    std::vector<unsigned char> v;

    while (ros::ok())
    {
        sensor_msgs::CameraInfoPtr info(new sensor_msgs::CameraInfo(info_mgr.getCameraInfo()));
        result = SN_GetOneFrame(board, mat.data, 64000, &fi);

        if (result != 0) {
            ROS_INFO("No frame available in frame buffer, retrying...");
            ros::spinOnce();
            loop_rate.sleep();
            continue;
        }

        Mat out_mat = imdecode(mat, 1);

        cv_image.image = out_mat;
        cv_image.encoding = "bgr8";

        sensor_msgs::ImagePtr image = cv_image.toImageMsg();

        // mat.data contains jpeg 
        v.assign(mat.ptr(), mat.ptr() + mat.total());
        cmpImage.data = v;

        if (info->K[0] != 0.0 &&
           (image->width != info->width
              || image->height != info->height)) {
            info.reset(new sensor_msgs::CameraInfo());
        }

        if (info->K[0] == 0.0) {
            info->width = image->width;
            info->height = image->height;
        }

        ros::Time time = ros::Time::now();

        info->header.stamp = time;
        info->header.frame_id = frame;
        image->header.stamp = time;
        image->header.frame_id = frame;
        cmpImage.header.stamp = time;
        cmpImage.header.frame_id = frame;

        info_pub.publish(info);
        image_pub.publish(image);
        cmp_image_pub.publish(cmpImage);

        ros::spinOnce();

        loop_rate.sleep();
    }

    SN_StopStream(board);
    SN_CloseBoard(board);

    return 0;
}
